# Edge Impulse - OpenMV Object Detection Example
import sensor, image, time,pyb,omv,math,utime,tf,lcd,gc
from pyb import UART,Pin,Timer,Servo
from machine import SPI
from umotor import UMOTOR
from pid import PID
from ultrasonic import ULTRASONIC
from button import BUTTON
from pycommon import PYCOMMON
common=PYCOMMON()
motor=UMOTOR()   #声明电机驱动，B0,B1,B4,B5
button=BUTTON()  #声明按键，梦飞openmv只有一个按键D8，因此直接内部指定了按键
count=0
lcd.init() # Initialize the lcd screen.
lcd.set_direction(2)
nn_input_sz=120
background_frame=sensor.alloc_extra_fb(nn_input_sz,nn_input_sz,sensor.GRAYSCALE)
dis_pid  = PID(p=0.3, i=0.01)
len_pid  = PID(p=0.10 ,i=0.01)

def background_update():#背景更新，可用于背景差分
    # 捕获若干帧用于计算背景帧(将捕获的图像与当前的背景帧进行混合，更新背景帧)
    img= sensor.snapshot()
    background_frame.draw_image(img,0,0)
    num_background_frames = 50  # 捕获50帧用于背景计算
    for i in range(num_background_frames):
        img = sensor.snapshot()
        #background_frame = background_frame.blend(img, alpha=128)
    return  background_frame.draw_image(img,0,0)
def frame_diffrence_update(img):
    global count
    largest_blob=None
    # 捕获当前帧
    #if count==0:
    #    background_frame.draw_image(img,0,0)
    #    frame_difference = img.difference(background_frame)
    #    count=1
    #    return None
    count=0
    # 计算当前帧和背景帧的灰度差
    frame_difference = img.difference(background_frame)
    # 对帧差进行阈值处理，生成运动掩码
    motion_mask = frame_difference.binary([(50, 255)])  # 阈值设置为50
    # 查找运动掩码中的轮廓
    #motion_mask.erode(1)
    motion_mask.dilate(2)
    blobs = motion_mask.find_blobs([(50, 255)], pixels_threshold=100, area_threshold=100, merge=True)
    # 如果找到轮廓，查找最大的轮廓
    if blobs:
        largest_blob = max(blobs, key=lambda b: b.pixels())
        # 在当前帧上绘制边界框
        img.draw_rectangle(largest_blob.rect())
        # 绘制中心点
        img.draw_cross(largest_blob.cx(), largest_blob.cy())
    lcd.display(img) # Take a picture and display the image.
    return largest_blob

def find_movement_control(img):
    label=None
    largest_blob=frame_diffrence_update(img)
    if largest_blob:
         object_s=10000/(largest_blob[2]*2) #计算距离
         if object_s>=70:
             common.run_forward(50,1000) #前进
             background_update()#背景更新，可用于背景差分
             print("go forward",object_s)
         if object_s<=50:
             common.run_back(50,1000) #后退
             background_update()#背景更新，可用于背景差分
             print("go back",object_s)
    else:
         motor.run(0,0)


##############################摄像头初始化部分#####################
sensor.reset()                         # Reset and initialize the sensor.
sensor.set_pixformat(sensor.GRAYSCALE)    # Set pixel format to RGB565 (or GRAYSCALE)
sensor.set_framesize(sensor.QQVGA)      # Set frame size to QVGA (320x240)
sensor.set_windowing((120, 120))       # Set 240x240 window.
clock = time.clock() # Tracks FPS. 设置一个定时器用来计算帧率
background_update()
# Find a face!
while (True):
    clock.tick()
    img = sensor.snapshot()
    find_movement_control(img)
    print(clock.fps(), "fps", end="\n\n")
    if button.state():
        click_timer=time.ticks()          #开始计时
        while button.state():  pass       #等待按键抬起
        if time.ticks()-click_timer>2000: #按键时长超过2s
            break                         #循环退出，回到主界面
    else :
        click_timer=time.ticks()#计时更新
